Add rangefinder using SITL and gazebo
Table of Content
Add rangefinder using SITL and gazebo#
Distance / Rangefinder has two type of message
- Distance sensor #132 (common)
- Rangefinder #173 (ardupilotmega) (reporting)
Distance sensor #132#
Distance sensor mavlink message - Distance sensor #132
Demo (using pymavlink)#
- Config SITL with RANGE FINDER
- Send mavlink distance using
pymavlink
params
RNGFND1_TYPE 10 # mavlink
RNGFND1_ORIENT 25 # down
RNGFND1_MAX_CM 1000 # cm
RNGFND1_MIN_CM 10 # cm
script
import time
from pymavlink import mavutil
from pymavlink.dialects.v20 import ardupilotmega
TO_MS = 1e3
UPDATE_RATE = 0.5
RNGFND_TYPE_MAVLINK = 10
SENSOR_ID = 1
SENSOR_MAX_CM = 1000
SENSOR_MIN_CM = 10
SENSOR_COVARIANCE = 0
SIM_CURRENT_READING_CM = 200
# Create the connection
master = mavutil.mavlink_connection("udp:127.0.0.1:14550")
# Wait a heartbeat before sending commands
master.wait_heartbeat()
t_start = time.time()
while True:
time.sleep(UPDATE_RATE)
boot_time = int((time.time() - t_start) * TO_MS)
master.mav.distance_sensor_send(
boot_time,
SENSOR_MIN_CM,
SENSOR_MAX_CM,
SIM_CURRENT_READING_CM,
ardupilotmega.MAV_DISTANCE_SENSOR_UNKNOWN,
SENSOR_ID,
ardupilotmega.MAV_SENSOR_ROTATION_PITCH_270,
SENSOR_COVARIANCE,
)
SITL/ sim_vehicle
./sim_vehicle.py -v ArduCopter \
-f quad -D \
--console \
--add-param-file /home/user/apm_ws/src/apm_bringup/config/range_finder.parm
params file
Add param file to sitl
--add-param-file <file path>
MAVROS#
Mavros has two plugins
- distance_sensor (common msg #132)
- rangefinder (ardupilot msg #173)
Demo#
- Send distance using distance_sensor plugin (msg #132)
- mavros open subscriber for each distance sensor declare as
subscriber: truesee config file example - Read data using rangefinder plugin (msg #173)
- Echo distance data from fcu using cli (/mavros/rangfinder_pub)
publish distance data#
demo send random distance data
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Range
TOPIC_DISTANCE_READ = "/mavros/rangefinder_pub"
TOPIC_DISTANCE_WRITE = "/mavros/rangefinder_sub"
TOPIC_APM_RANGEFINDER = "/mavros/rangefinder/rangefinder"
DRONE_NO = 1
MIN_RANGE = 0.0
MAX_RANGE = 4.0
RANGE_SENSOR_TYPE = 1
SENSOR_ID = 1
COVARIANCE = 0
PUB_INTERVAL = 1/10
class RangeFinderNode(Node):
def __init__(self):
node_name = "range_finder"
super().__init__(node_name)
#mavros open subscriber, our node pub to it
self.__range_pub = self.create_publisher(
Range, TOPIC_DISTANCE_WRITE, qos_profile=qos_profile_sensor_data
)
self.create_subscription(
Range, TOPIC_APM_RANGEFINDER, self.__apm_rangefinder_message_handler, qos_profile=qos_profile_sensor_data
)
self.create_timer(PUB_INTERVAL, self.__send_range_message)
def __apm_rangefinder_message_handler(self, msg: Range):
self.get_logger().info(f"apm rangefinder: {msg.range}")
def __send_range_message(self, distance=2.0):
sec, nanosec = self.get_clock().now().seconds_nanoseconds()
range_msg = Range()
range_msg.header.frame_id = "rangefinder"
range_msg.header.stamp.sec = sec
range_msg.header.stamp.nanosec = nanosec
range_msg.range = float(distance)
range_msg.radiation_type = RANGE_SENSOR_TYPE
range_msg.min_range = MIN_RANGE
range_msg.max_range = MAX_RANGE
self.__range_pub.publish(range_msg)
def main(args=None):
rclpy.init(args=args)
node = RangeFinderNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
print("User exit")
finally:
node.destroy_node()
rclpy.try_shutdown()
if __name__ == "__main__":
main()
echo distance data#
ros2 topic echo /mavros/rangefinder_pub
config parma
config param expend settings as YAML
config multiple sensors#
/mavros/**/distance_sensor:
ros__parameters:
config: |
rangefinder_pub:
id: 0
frame_id: "lidar"
#orientation: PITCH_270 # sended by FCU
field_of_view: 0.0
send_tf: false
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
rangefinder_sub:
subscriber: true
id: 1
orientation: PITCH_270
rangefinder_fwd:
subscriber: true
id: 2
orientation: PITCH_180
orientation
declare at /mavros/src/lib/enum_sensor_orientation.cpp
Demo to delete#
sitl
sim_vehicle.py -v ArduCopter -f gazebo-iris -A "--defaults /home/user/wasp_ws/src/wasp_bringup/config/copter.parm,/home/user/wasp_ws/src/wasp_bringup/config/gazebo-iris.parm" -I0 -m "--out=127.0.0.1:14552" -m "--load-module graph"
